/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_IMUSENSORVISUALISATION_H_
#define __MMM_IMUSENSORVISUALISATION_H_

#include "../../SensorVisualisation.h"
#include <MMM/Motion/Plugin/IMUPlugin/IMUSensor.h>

#include <Inventor/nodes/SoSeparator.h>
#include <VirtualRobot/RobotConfig.h>

namespace MMM
{

class IMUSensorVisualisation : public SensorVisualisation
{
public:
    IMUSensorVisualisation(IMUSensorPtr sensor, VirtualRobot::RobotPtr robot, SoSeparator* sceneSep);

    std::string getType();

    int getPriority();

private:
    void update(float timestep, float delta);
    void update(float timestep);
    void update(IMUSensorMeasurementPtr measurement);

    void setPose(SoSeparator* s, const Eigen::Vector3f &pose);
    SoSeparator* CreateCoordSystemVisualization(const Eigen::Vector3f &acceleration, float scaling = 0.5f, float axisSize = 10.0f, int nrOfBlocks = 5);

    IMUSensorPtr sensor;
    VirtualRobot::RobotPtr robot;
};

}

#endif
